// --------------------------------------------------------------------------------------------------------------------
// <copyright file="NavBenchmark.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Navigation.Benchmark
{
    using System;
    using System.Collections.Generic;
    using System.Diagnostics;
    using System.IO;
    using System.Linq;
    using System.Net.Sockets;
    using System.Runtime.InteropServices;
    using System.Threading;
    using System.Windows.Media; 
    using CmdLine;

    using Microsoft.Robotics.IO;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Vision;
    using Microsoft.Robotics.Vision.Cameras.Webcam;
    using Microsoft.Robotics.Vision.Runtime.Cameras;
    
    /// <summary>
    /// The core working class for the Navigation Benchmark
    /// </summary>
    public class NavBenchmark : IDisposable
    {
        /// <summary>
        /// The name of the ground truth file
        /// </summary>
        private const string GroundTruthFileName = "groundTruth.xml";
        
        /// <summary>
        /// The name of the distances file
        /// </summary>
        private const string DistancesFileName = "distances.xml";

        /// <summary>
        /// The name of the results file
        /// </summary>
        private const string NavOutputFilename = "results.txt";

        /// <summary>
        /// The name of the results file
        /// </summary>
        private const string LogFilename = "log.txt";

        /// <summary>
        /// Image width
        /// </summary>
        private const int ImageWidth = 1280;

        /// <summary>
        /// Image height
        /// </summary>
        private const int ImageHeight = 720;

        /// <summary>
        /// Frame rate numerator
        /// </summary>
        private const int FrameRateNumerator = 2;

        /// <summary>
        /// Frame rate denominator
        /// </summary>
        private const int FrameRateDenominator = 1;

        /// <summary>
        /// Camera Sub-Type
        /// </summary>
        private const string CameraSubType = "YUY2";

        /// <summary>
        /// exposure list to try. Null means use Auto Exposure.  Use multiple nulls to allow time for exposure to settle.
        /// </summary>
        private int?[] exposures = { null, null, null, null, null, 1, 0, -1, -2, -3, -4, -5, -6, -7, -8, -9, -10 };

        /// <summary>
        /// save the auto frame for outputting in case none produce a pose.
        /// </summary>
        private volatile ImageFrameYuy2 imageFrameAuto;

        /// <summary>
        /// the current index into the exposure list.
        /// </summary>
        private volatile int currentExposureIndex;

        /// <summary>
        /// Unique run identifier
        /// </summary>
        private long runId;

        /// <summary>
        /// List of ground truth poses
        /// </summary>
        private List<Pose2D> groundTruthPoses = new List<Pose2D>();

        /// <summary>
        /// The results of comparing current pose with original pose
        /// </summary>
        private List<MetricResult> metricsResults = new List<MetricResult>();

        /// <summary>
        /// An array of distances from every landmark to every other landmark.
        /// </summary>
        private double[][] distances;

        /// <summary>
        /// Set to true when signaled during a navigation or mapping
        /// </summary>
        private volatile bool waitingForImage = false;

        /// <summary>
        /// Set to true when signaled after capturing a pose during navigation
        /// </summary>
        private volatile bool waitingForNavStart = false;

        /// <summary>
        /// Name of the output file
        /// </summary>
        private string fileOutName;

        /// <summary>
        /// The mode the app is running in
        /// </summary>
        private string mode;

        /// <summary>
        /// The start time of the run
        /// </summary>
        private DateTime runStartTime;

        /// <summary>
        /// The TCP receiver
        /// </summary>
        private TcpCom tcpCom;

        /// <summary>
        /// The RGB Camera
        /// </summary>
        private RGBCameraConnect camera;

        /// <summary>
        /// The ground truth estimator
        /// </summary>
        private GroundTruthEstimator groundTruthEstimator;

        /// <summary>
        /// the number of landmarks (from the landmarks calibration file)
        /// </summary>
        private int landmarkCount;

        /// <summary>
        /// The starting landmark ID
        /// </summary>
        private int startlLandmarkId;

        /// <summary>
        /// The current landmark ID
        /// </summary>
        private int currentLandmarkId;

        /// <summary>
        /// Stopwatch for timing navigation legs
        /// </summary>
        private Stopwatch stopwatch;

        /// <summary>
        /// Output directory
        /// </summary>
        private string outDir;

        /// <summary>
        /// The input directory
        /// </summary>
        private string inDir;

        /// <summary>
        /// The camera focus
        /// </summary>
        private int? focus;

        /// <summary>
        /// The requested camera exposure
        /// </summary>
        private int? exposure;

        /// <summary>
        /// The requested camera string
        /// </summary>
        private string cameraString;

        /// <summary>
        /// The requested threshold for distance error in millimeters
        /// </summary>
        private double? distanceErrorThreshold;

        /// <summary>
        /// The port to listen on
        /// </summary>
        private int port;

        /// <summary>
        /// An object for locking
        /// </summary>
        private object lockObject = new object();

        /// <summary>
        /// Track whether Dispose has been called.
        /// </summary>
        private bool disposed;

        /// <summary>
        /// Initializes a new instance of the <see cref="NavBenchmark" /> class.
        /// </summary>
        /// <param name="mode">The mode the benchmark is running in</param>
        /// <param name="port">The socket port to listen on</param>
        /// <param name="inDir">The input directory</param>
        /// <param name="outDir">The output directory</param>
        /// <param name="focus">The camera focus parameter</param>
        /// <param name="exposure">The camera exposure parameter</param>
        /// <param name="camera">The regex match string for the camera</param>
        /// <param name="distanceErrorThreshold">The maximum allowed distance error</param>
        public NavBenchmark(string mode, int port, string inDir, string outDir, int? focus, int? exposure, string camera, int? distanceErrorThreshold)
        {
            this.runId = 0;
            this.startlLandmarkId = 0;
            this.port = port;
            this.outDir = outDir;
            this.inDir = inDir;
            this.mode = mode;
            this.focus = focus;
            this.exposure = exposure;
            this.cameraString = camera;
            this.distanceErrorThreshold = distanceErrorThreshold;
        }

        /// <summary>
        /// Initializes the class
        /// </summary>
        /// <returns>Boolean indicating success or failure</returns>
        public bool Initialize()
        {
            // setup the listening port
            this.tcpCom = new TcpCom();

            // the CommandLine class ensures the argumets are parsable so this is safe use.
            this.tcpCom.StartListening(this.port, this.OnSignaled);

            // initialize the pose estimator
            this.groundTruthEstimator = new GroundTruthEstimator(this.inDir);
            if (!this.groundTruthEstimator.HasCalibrationInfo)
            {
                this.TextOut("Could not initilaize the ground truth estimator.");
                this.TextOut("Check that the configuration folder has the necessary configuration files.");
                return false;
            }

            this.landmarkCount = this.groundTruthEstimator.LandmarkCount;
            this.TextOut("{0} Landmarks listed in the landmark configuration file", this.landmarkCount);

            // initialize for current mode
            switch (this.mode)
            {
                case "calib":
                    this.InitializeCalibrationMode();
                    break;
                case "map":
                    this.InitializeMappingMode(this.outDir);
                    break;
                case "nav":
                    if (!this.TryInitializeNavigationMode(this.inDir))
                    {
                        this.TextOut("Failed to initialize in Navigation mode.");
                        return false;
                    }

                    break;
                default:
                    this.TextOut("Unrecognized mode specified with /m option.");
                    this.ShowHelp();
                    return false;
            }

            this.runStartTime = DateTime.Now;
            return true;
        }

        /// <summary>
        /// Called by the client to signal the benchmark
        /// </summary>
        /// <param name="buffer">The received buffer</param>
        public void OnSignaled(byte[] buffer)
        {
            lock (this.lockObject)
            {
                switch (this.mode)
                {
                    case "map":
                        if (this.currentLandmarkId < this.landmarkCount)
                        {
                            // always start a capture with Auto and then try the rest
                            this.InitializeCamera();

                            // we don't care about the contents of the message.
                            this.waitingForImage = true;
                        }

                        break;

                    case "nav":
                        if (this.waitingForNavStart)
                        {
                            Message message = this.ParseMessage(buffer);
                            this.runId = message.RunId;
                            this.startlLandmarkId = message.LandmarkId;
                            this.TextOut("Start message received from robot: Landmark {0}.  Starting navigation timer.", message.LandmarkId);
                            this.TextOut(" Signal when robot arrives, or Press <esc> to stop the run.");
                            this.stopwatch.Restart();

                            string navOutputPath = Path.Combine(this.outDir, NavOutputFilename);
                            if (!File.Exists(navOutputPath))
                            {
                                // Create a file to write to. 
                                using (StreamWriter resultFile = File.CreateText(navOutputPath))
                                {
                                    resultFile.WriteLine("runId\tRunStartTime\tLandmarkStartId\tLandmarkEndId\tNavTime(s)\tNavDistance(m)\tPositionError(m)\tHeadingError(rad)");
                                }
                            }

                            this.waitingForNavStart = false;
                        }
                        else
                        {
                            // always start a capture with Auto and then try the rest
                            this.ActivateCamera();
                            this.InitializeCamera();
                            
                            Message message = this.ParseMessage(buffer);
                            this.runId = message.RunId;
                            this.currentLandmarkId = message.LandmarkId;
                            waitingForImage = true;
                            waitingForNavStart = true;
                            TextOut("Arrival signal received from robot: Landmark {0}. Stopping navigation timer.", message.LandmarkId);
                            this.TextOut(" Activating Camera and scanning for landmark.");
                            stopwatch.Stop();
                        }

                        break;
                }
            }
        }

        /// <summary>
        /// Shutdown the navigation benchmark
        /// </summary>
        public void Shutdown()
        {
            // allow time for the final image to arrive and be processed if they exited immediatly after signalling the final capture
            while (this.waitingForImage)
            {
                Thread.Sleep(2 * ((FrameRateNumerator / FrameRateDenominator) / 60 * 1000));
            }

            this.DeactivateCamera();

            if (this.mode == "map" && this.groundTruthPoses.Count > 0)
            {
                this.TextOut("Ground truth file created at {0}", Path.Combine(this.outDir, GroundTruthFileName));
            }

            this.tcpCom.StopListening();
            this.groundTruthEstimator.Dispose();
        }

        /// <summary>
        /// Implement IDisposable
        /// </summary>
        public void Dispose()
        {
            this.Dispose(true);
            GC.SuppressFinalize(this);
        }

        /// <summary>
        /// Dispose resources.
        /// </summary>
        /// <param name="disposing">True if dispose is called by user code, false otherwise</param>
        protected virtual void Dispose(bool disposing)
        {
            // Check to see if Dispose has already been called. 
            if (!this.disposed)
            {
                if (disposing)
                {
                    if (this.groundTruthEstimator != null)
                    {
                        this.groundTruthEstimator.Dispose();
                        this.groundTruthEstimator = null;
                    }
                }

                this.disposed = true;
            }
        }

        /// <summary>
        /// Initialize parts needed for mapping
        /// </summary>
        /// <param name="inputFolder">Path to the folder containing input files</param>
        /// <returns>True if initialization was successful</returns>
        private bool TryInitializeNavigationMode(string inputFolder)
        {
            this.TextOut("Starting in Navigation mode.");
            this.TextOut("In Navigation mode the robot is expected to navigate autonomously from landmark to landmark.");
            this.TextOut("The user navigation application must signal over TCP/IP when starting to navigate, and upon arrival.");

            this.fileOutName = NavOutputFilename;
            
            string groundTruthFilePath = Path.Combine(inputFolder, GroundTruthFileName);
            this.groundTruthPoses = DataLoader<List<Pose2D>>.ImportData(groundTruthFilePath);
            if (this.groundTruthPoses == null)
            {
                this.TextOut("The {0} file could not be found or is not the correct format. This file is generated by running this app in mapping mode '/mode:map'", groundTruthFilePath);
                return false;
            }

            if (this.groundTruthPoses.Count < this.landmarkCount)
            {
                this.TextOut("The {0} does not contain ground truth entries for all the landmarks.", groundTruthFilePath);
                return false;
            }

            string distancesFilePath = Path.Combine(inputFolder, DistancesFileName);
            this.distances = DataLoader<double[][]>.ImportData(distancesFilePath);
            if (this.distances == null)
            {
                this.TextOut("The {0} file could not be found or is not the correct format.", distancesFilePath);
                return false;
            }

            if (this.landmarkCount != this.distances.Length)
            {
                this.TextOut("The distances file {0} must be an NxN matrix where N is the same as the number of landmarks in the Landmark Info file {1}", distancesFilePath, Microsoft.Robotics.Vision.Runtime.GroundTruthExtraction.CameraCalibrationInfo.Filename);
            }

            foreach (double[] d in this.distances)
            {
                if (this.landmarkCount != this.distances.Length)
                {
                    this.TextOut("The distances file {0} must be an NxN matrix where N is the same as the number of landmarks in the Landmark Info file {1}", distancesFilePath, Microsoft.Robotics.Vision.Runtime.GroundTruthExtraction.CameraCalibrationInfo.Filename);
                }
            }

            this.stopwatch = new Stopwatch();

            this.TextOut("Position the robot under the starting landmark and signal when you begin autonomous navigation to the first waypoint. Signal again when you arrive.");
            this.waitingForNavStart = true;
            return true;
        }

        /// <summary>
        /// Initialize parts needed for navigation metrics
        /// </summary>
        /// <param name="outDir">The output folder</param>
        private void InitializeMappingMode(string outDir)
        {
            this.TextOut("Starting in Mapping mode.");
            this.TextOut("In Mapping mode you are expected to drive the robot manually to each landmark in numeric order.");
            this.TextOut("Drive to the first landmark (ID=0) and signal to capture the pose.");
            this.TextOut("[Press <space> or send a message to the specified port to signal.");
            this.TextOut("[Press <esc> when you are finished visiting all the landmarks.");
            this.fileOutName = GroundTruthFileName;

            string groundTruthPath = Path.Combine(outDir, this.fileOutName);
            if (File.Exists(groundTruthPath))
            {
                this.TextOut(string.Empty);
                this.TextOut("******************************************");
                this.TextOut("WARNING: a ground truth file already exists at {0}.", groundTruthPath);
                this.TextOut("If you continue, this file will be overwritten.");
                this.TextOut("Press <esc> now to exit now.");
                this.TextOut("******************************************");
            }

            // startup the camera
            this.ActivateCamera();
        }

        /// <summary>
        /// Initialize parts needed for calibration
        /// </summary>
        private void InitializeCalibrationMode()
        {
            this.TextOut("Starting in Calibration mode.\nSee \"Navigation Benchmark.pdf\" for calibration instructions.");
            this.fileOutName = GroundTruthFileName;

            // startup the camera
            this.ActivateCamera();
        }

        /// <summary>
        /// Displays help information
        /// </summary>
        private void ShowHelp()
        {
            this.TextOut("'NavBenchmark /?' to show help");
        }

        /// <summary>
        /// Called when a byte is received from the client.
        /// </summary>
        /// <param name="buffer">A buffer containing the bytes sent from the client</param>
        /// <returns>A decoded message</returns>
        private Message ParseMessage(byte[] buffer)
        {
            return new Message() { LandmarkId = buffer[0], RunId = BitConverter.ToInt64(buffer, 1) };
        }

        /// <summary>
        /// Activate the camera.
        /// </summary>
        private void ActivateCamera()
        {
            this.camera = new RGBCameraConnect(ImageWidth, ImageHeight, FrameRateNumerator, FrameRateDenominator, CameraSubType, this.cameraString, this.RGBImageReady);
            this.camera.InitializeCamera();
            this.camera.CaptureSample();
        }

        /// <summary>
        /// Sets the default camera properties
        /// </summary>
        private void InitializeCamera()
        {
            this.currentExposureIndex = 0;

            if (null != this.exposure)
            {
                this.camera.SetProperty(ManagedCameraControlProperty.Exposure, (int)this.exposure, ManagedCameraControlPropertyFlags.Manual);
            }
            else
            {
                this.camera.SetProperty(ManagedCameraControlProperty.Exposure, 0, ManagedCameraControlPropertyFlags.Auto);
            }

            if (null != this.focus)
            {
                if (!this.camera.SetProperty(ManagedCameraControlProperty.Focus, (int)this.focus, ManagedCameraControlPropertyFlags.Manual))
                {
                    Console.WriteLine("Unable to set camera focus. Focus may not be a settable property on this camera.");
                }
            }
        }

        /// <summary>
        /// Deactivate the camera.
        /// </summary>
        private void DeactivateCamera()
        {
            if (this.camera != null)
            {
                if (this.camera.Camera != null)
                {
                    this.camera.Camera.Shutdown();
                }

                this.camera.UninitializeCamera();
                this.camera = null;
            }
        }

        /// <summary>
        /// Called when a camera image is ready and processes it to attempt to find the robot pose from a visible landmark.
        /// The landmark might not be visible because it is not in view of the camera, or because the lighting conditions.
        /// If the pose is not found the method will advance the current exposure index so the next image will have a different exposure.
        /// When all exposures in the list are tried it will loop back to the first exposure index.
        /// If all exposures have been tried and "waitingForImage" is true, the "auto" exposed image will be saved to disk for 
        /// diagnostic purposes.
        /// "waitingForImage" is true when the client has signaled it is time to record the pose. 
        /// In map mode a message with current pose (or no pose) is displayed continuously (updated with each image)
        /// In nav mode the current exposure is displayed when "waitingForImage" is true and new exposures are being tried.
        /// </summary>
        /// <param name="imageFrame">RGB Image Frame (formatted as Yuy2)</param>
        private void RGBImageReady(ImageFrameYuy2 imageFrame)
        {
            lock (this.lockObject)
            {
                Pose2D pose = new Pose2D();
                Vector3 cameraTranslations = new Vector3();
                bool foundPose = false;
                RGBCameraProperty[] cameraProperties;

                if (!this.groundTruthEstimator.HasCalibrationInfo)
                {
                    return;
                }

                if (this.exposures[this.currentExposureIndex] == null && this.waitingForImage)
                {
                    // save this (auto exposed image) in case we need to write it to disk when all exposures fail.
                    this.imageFrameAuto = imageFrame;
                }

                foundPose = this.groundTruthEstimator.TryGetGroundTruthFromImage(imageFrame, this.currentLandmarkId, out pose, out cameraTranslations);

                if (this.waitingForImage)
                {
                    if (!foundPose && (this.currentExposureIndex == this.exposures.Count() - 1))
                    {
                        // tried all exposures and found nothing
                        // save "Auto" Exposure image for diagnostics viewing 
                        ImageOperations2D.SaveImageToFile(ImageOperations2DInterop.ConvertYUY2ToGray8Image(this.imageFrameAuto), Path.GetRandomFileName() + " Landmark " + this.currentLandmarkId + "_Fail.bmp");
                        this.ProcessPose(pose);
                        this.waitingForImage = false;
                    }

                    if (foundPose)
                    {
                        ImageOperations2D.SaveImageToFile(ImageOperations2DInterop.ConvertYUY2ToGray8Image(imageFrame), Path.GetRandomFileName() + string.Format(" Landmark {0} ({1:f3}, {2:f3}, {3:f4}), [exposure={4}]", this.currentLandmarkId, pose.X, pose.Y, pose.Heading, this.GetExposureString()) + ".bmp");
                        this.ProcessPose(pose);
                    }
                }

                if ((this.mode == "map" || this.mode == "calib") && this.currentLandmarkId < this.landmarkCount)
                {
                    int currentTop = Console.CursorTop;

                    if (foundPose)
                    {
                        if (this.mode == "map")
                        {
                            Console.WriteLine("\nCurrent Pose: ({0:f3}, {1:f3}, {2:f4}), [exposure:{3}]       ", pose.X, pose.Y, pose.Heading, this.GetExposureString());
                        }
                        else
                        {
                            Console.WriteLine("\nTranslations: X:{0:f3}, Y:{1:f3}, Z:{2:f4}), [exposure:{3}]       ", cameraTranslations.X * MathConstants.MilliunitsPerUnit, cameraTranslations.Y * MathConstants.MilliunitsPerUnit, cameraTranslations.Z * MathConstants.MilliunitsPerUnit, this.GetExposureString());
                        }
                    }
                    else
                    {
                        Console.WriteLine("\nCurrent Pose: Landmark not visible.   [exposure:{0}]         ", this.GetExposureString());
                    }

                    Console.SetCursorPosition(0, currentTop);
                }

                if (this.mode == "nav" && this.waitingForImage == true)
                {
                    Console.WriteLine("Trying Exposure: {0}", this.GetExposureString());
                }

                if (!foundPose && (this.mode == "map" || this.mode == "calib" || (this.mode == "nav"  && this.waitingForImage == true)))
                {
                    if (null == this.exposure)
                    {
                        if (this.exposures[this.currentExposureIndex] == null)
                        {
                            cameraProperties = new RGBCameraProperty[] { new RGBCameraProperty(ManagedCameraControlProperty.Exposure, 0, ManagedCameraControlPropertyFlags.Auto) };
                        }
                        else
                        {
                            cameraProperties = new RGBCameraProperty[] { new RGBCameraProperty(ManagedCameraControlProperty.Exposure, (int)this.exposures[this.currentExposureIndex], ManagedCameraControlPropertyFlags.Manual) };
                        }

                        this.camera.SetProperties(cameraProperties);
                        
                        this.currentExposureIndex++;
                        if (this.currentExposureIndex >= this.exposures.Count())
                        {
                            this.currentExposureIndex = 0; 
                        }
                    }
                }

                if (foundPose)
                {
                    this.waitingForImage = false;
                }
            }
        }

        /// <summary>
        /// Create a string that represents the current exposure
        /// </summary>
        /// <returns>A string representation of the exposure</returns>
        private string GetExposureString()
        {
            if (null == this.exposure)
            {
                return this.exposures[this.currentExposureIndex] == null ? "Auto " + this.currentExposureIndex.ToString() : this.exposures[this.currentExposureIndex].ToString();
            }
            else
            {
                return this.exposure.ToString();
            }
        }

        /// <summary>
        /// Process a new pose.
        /// </summary>
        /// <param name="pose">The pose to process. Pose is the delta position.</param>
        private void ProcessPose(Pose2D pose)
        {
            switch (this.mode)
            {
                case "map":
                    if (!double.IsNaN(pose.X))
                    {
                        this.AddGroundTruthPose(pose);
                        Console.Beep(800, 200);
                        this.TextOut("\n\n*****************************************************************************");
                        this.TextOut("Landmark Aquired.  Position recorded.\nNow drive to the next landmark (ID={0}) and signal.", this.currentLandmarkId + 1);
                        this.TextOut("*****************************************************************************\n");
                        this.currentLandmarkId++;

                        if (this.currentLandmarkId >= this.landmarkCount)
                        {
                            this.TextOut("*** All landmarks visited. Press <esc> to exit. ***");
                        }

                        // let the client know the landmark has been processed successfully
                        this.tcpCom.SendReply(1);
                    }
                    else
                    {
                        Console.Beep(400, 1000);
                        this.TextOut("\n\n*****************************************************************************");
                        this.TextOut("FAILED TO READ LANDMARK.\nDuring mapping this is usually caused by poor lighting.\nYou can signal to try again.");
                        this.TextOut("*****************************************************************************\n");

                        // let the client know the landmark was not visible
                        this.tcpCom.SendReply(0);
                    }

                    break;
                case "nav":
                    this.AddWaypointMetric(pose);
                    this.TextOut("Signal when you begin navigation to the next landmark and signal again when you arrive.");
                    this.waitingForNavStart = true;
                    this.waitingForImage = false;
                    this.DeactivateCamera();
                    break;
            }
        }

        /// <summary>
        /// Add the current pose to the list of ground truth poses
        /// </summary>
        /// <param name="pose">The current pose</param>
        private void AddGroundTruthPose(Pose2D pose)
        {
            this.groundTruthPoses.Add(pose);
            this.TextOut("\nCaptured Pose: ({0:f3}, {1:f3}, {2:f4}) [exposure:{3}]", pose.X, pose.Y, pose.Heading, this.GetExposureString());
            DataLoader<List<Pose2D>>.ExportData(Path.Combine(this.outDir, this.fileOutName), this.groundTruthPoses);
            this.InitializeCamera();
        }

        /// <summary>
        /// Use the current pose to calculate the metrics for this waypoint
        /// </summary>
        /// <param name="pose">The current pose</param>
        private void AddWaypointMetric(Pose2D pose)
        {
            double deltaX = double.NaN;
            double deltaY = double.NaN;
            double deltaHeading = double.NaN;
            double deltaPos;

            MetricResult result;

            if (double.IsNaN(pose.X))
            {
                result = new MetricResult { LandmarkStart = this.startlLandmarkId, LandmarkEnd = this.currentLandmarkId, PositionError = double.NaN, HeadingError = double.NaN, Time = double.NaN, Distance = double.NaN };
                this.WriteNavMetric(result);
                Console.Beep(400, 1000);
                this.tcpCom.SendReply(0);
            }
            else
            {
                deltaX = pose.X - this.groundTruthPoses[this.currentLandmarkId].X;
                deltaY = pose.Y - this.groundTruthPoses[this.currentLandmarkId].Y;
                deltaPos = Math.Sqrt((deltaX * deltaX) + (deltaY * deltaY)) / MathConstants.MilliunitsPerUnit;
                deltaHeading = pose.SmallestCircularDeltaAngle(this.groundTruthPoses[this.currentLandmarkId].Heading);

                if (this.distanceErrorThreshold == null || (deltaPos * MathConstants.MilliunitsPerUnit) <= this.distanceErrorThreshold)
                {
                    result = new MetricResult { LandmarkStart = this.startlLandmarkId, LandmarkEnd = this.currentLandmarkId, PositionError = deltaPos, HeadingError = deltaHeading, Time = this.stopwatch.ElapsedMilliseconds, Distance = this.distances[this.startlLandmarkId][this.currentLandmarkId] };
                    this.WriteNavMetric(result);
                    Console.Beep(800, 200);
                    this.tcpCom.SendReply(1);
                }
                else
                {
                    this.TextOut("\n\n**************************************************");
                    this.TextOut("Distance error ({0:f3}m) is larger than allowed threshold ({1}mm). Failure row entered into {2}", deltaPos, this.distanceErrorThreshold, NavOutputFilename);
                    this.TextOut("**************************************************\n");

                    result = new MetricResult { LandmarkStart = this.startlLandmarkId, LandmarkEnd = this.currentLandmarkId, PositionError = double.NaN, HeadingError = double.NaN, Time = double.NaN, Distance = double.NaN };
                    this.WriteNavMetric(result);
                    Console.Beep(400, 1000);
                    this.tcpCom.SendReply(0);
                }
            }

            this.TextOut(" Starting Landmark ID: {0}", this.startlLandmarkId);
            this.TextOut(" Ending Landmark ID: {0}", this.currentLandmarkId);
            this.TextOut("\tDelta Pose: ({0:f3}, {1:f3}, {2:f4})", deltaX, deltaY, deltaHeading);
            this.TextOut("\tDistance Error: {0:f3}", Math.Sqrt((deltaX * deltaX) + (deltaY * deltaY)) / MathConstants.MilliunitsPerUnit);
            this.TextOut("\tElapsed Time: {0}", result.Time / MathConstants.MilliunitsPerUnit);
            this.TextOut("\tDistance: {0}", result.Distance);
        }
       
        /// <summary>
        /// Writes an entry to the navigation metrics result file
        /// </summary>
        /// <param name="result">The result to write</param>
        private void WriteNavMetric(MetricResult result)
        {
            string navOutputPath = Path.Combine(this.outDir, NavOutputFilename);

            using (StreamWriter resultFile = File.AppendText(navOutputPath))
            {
                resultFile.WriteLine(
                    "{0}\t{1}\t{2}\t{3}\t{4}\t{5}\t{6:f3}\t{7:f4}",
                    this.runId,
                    this.runStartTime,
                    result.LandmarkStart,
                    result.LandmarkEnd,
                    result.Time / MathConstants.MilliunitsPerUnit,
                    result.Distance,
                    result.PositionError,
                    result.HeadingError);
            }

            this.TextOut(" Result written to: {0}", navOutputPath);
        }

        /// <summary>
        /// Write output to the screen and the log file
        /// </summary>
        /// <param name="format">The string format</param>
        /// <param name="args">Optional args</param>
        private void TextOut(string format, params object[] args)
        {
            Console.WriteLine(format, args);
            if (!string.IsNullOrEmpty(format))
            {
                string logPath = Path.Combine(this.outDir, LogFilename);
                if (!File.Exists(logPath))
                {
                    if (!Directory.Exists(this.outDir))
                    {
                        Directory.CreateDirectory(this.outDir);
                    }

                    using (StreamWriter logFile = File.CreateText(logPath))
                    {
                        logFile.Write(DateTime.Now + " ");
                        logFile.WriteLine("Starting new logfile");
                    }
                }

                using (StreamWriter logFile = File.AppendText(logPath))
                {
                    logFile.Write(DateTime.Now + " ");
                    logFile.WriteLine(format, args);
                }
            }
        }

        /// <summary>
        /// Structure for a TCP navigation signal message
        /// </summary>
        [StructLayout(LayoutKind.Sequential, Pack = 1)]
        public struct Message
        {
            /// <summary>
            /// A landmark ID indicating the starting landmark
            /// </summary>
            public byte LandmarkId;

            /// <summary>
            /// A unique run identifier
            /// </summary>
            public long RunId;
        }

        /// <summary>
        /// Result at each waypoint
        /// </summary>
        private struct MetricResult
        {
            /// <summary>
            /// Gets or sets the starting landmark
            /// </summary>
            public int LandmarkStart { get; set; }

            /// <summary>
            /// Gets or sets the ending landmark
            /// </summary>
            public int LandmarkEnd { get; set; }

            /// <summary>
            /// Gets or sets the position distance error in mm
            /// </summary>
            public double PositionError { get; set; }

            /// <summary>
            /// Gets or sets the heading error in radians
            /// </summary>
            public double HeadingError { get; set; }

            /// <summary>
            /// Gets or sets the distance travelled
            /// </summary>
            public double Distance { get; set; }

            /// <summary>
            /// Gets or sets the elapsed time 
            /// </summary>
            public double Time { get; set; }
        }
    }
}
